//
//  Map.c
//  Vector_Math
//
//  Created by John R Peterson on 3/13/12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//


#include "Map.h"


// where the resolution is how many blocks in the map correspond to one unit in the real world
// and where sizeW is the span of the map in real world coordinates
// this matrix will be stupidly big so don't go making a bunch of these
// TODO instantiate not as empty but as unexplored and then need a function to initialize cells as empty
// when they have been observed
RMap* newRMap(float resolution, float sizeW) {
    
    // create map
    RMap *this = (RMap *) malloc(sizeof(RMap));
    if (this == NULL) {
        printf("Warning malloc failed in newMap\n");
        exit(1);
    }
    // remember resolution
    this->resolution = resolution;
    // instantiate the matrix to represent the map
    int size = ceilf(sizeW*resolution);
    this->grid = Matrix_createZeros(size, size);
    // instantiate transformation between real world coordinates and grid coordinates
    this->Twtom = Matrix_scaleI(Matrix_createRBT2D(size/2.0 - 0.5, size/2.0 - 0.5, PI/2.0),resolution);
    
    return this;
}

// uses wu's algorithm http://en.wikipedia.org/wiki/Xiaolin_Wu%27s_line_algorithm to draw an anti-aliased line
// its nice because it handles starting and stopping on non-integer pixeles elegantly
void RMap_drawLine(RMap *this, Line *l) {
    
    // get start and stop points
    Line_print(l);
    float x1,y1,x2,y2;
    Vec *start = Vec_addI(Vec_scale(l->direction,-(l->length)/2.0),l->centroid);
    Vec *end = Vec_addI(Vec_scale(l->direction,(l->length)/2.0),l->centroid);
    // transform these points into matrix coordinates
    start = Matrix_multiplyAV_D(this->Twtom,Vec_toHI(start));
    end = Matrix_multiplyAV_D(this->Twtom, Vec_toHI(end));
    
    x1 = start->val[0];
    y1 = start->val[1];
    x2 = end->val[0];
    y2 = end->val[1];
    Vec_destroy(start); // clean up after ourselves
    Vec_destroy(end);
    
    int rows = this->grid->rows;
    int columns = this->grid->columns;
    // if the end points are within the matrix, then don't need to do any bounds checking
    if ((x1 < rows) && (x1 >= 0) && (x2 < rows) && (x2 >= 0) && (y1 < columns) && (y1 >= 0) && (y2 < columns) && (y2 >= 0)) {
        RMap_drawLineP2P(this, x1, y1, x2, y2);
    } else {
        RMap_drawLineP2PSAFE(this, x1, y1, x2, y2);
    }
}


void RMap_drawLineP2P(RMap *this, float x1, float y1, float x2, float y2) {
    
    float **M = this->grid->val;
    
    short flag = TRUE;
    float dx,dy, gradient;
    dx = x2 - x1;
    dy = y2 - y1;
    if (fabsf(dx) < fabsf(dy)) {
        swap(&x1,&y1);
        swap(&x2,&y2);
        swap(&dx,&dy);
        flag = FALSE;
    }
    if (x2 < x1) {
        swap(&x1,&x2);
        swap(&y1,&y2);
    }
    gradient = dy/dx;
    
    printf("Do these look right 2 [ %f, %f] [%f, %f]\n",x1,y1,x2,y2);
    // handle the first endpoint
    int xpxl1,ypxl1;
    float xend,yend, xgap, intery;
    xend = roundf(x1);
    yend = y1 + gradient * (xend - x1);
    xgap = rfpart(x1 + 0.5);
    xpxl1 = xend; 
    ypxl1 = ipart(yend);
    // plot some stuff for this 
    printf("xend = %f, yend = %f, xgap = %f, gradient = %f\n",xend,yend,xgap, gradient);
    printf("xpxl1 = %d, ypxl1 = %d\n",xpxl1,ypxl1);
    
    if (flag) {
        M[xpxl1][ypxl1] = rfpart(yend) * xgap;
        M[xpxl1][ypxl1+1] = fpart(yend) * xgap;
    } else {
        M[ypxl1][xpxl1] = rfpart(yend) * xgap;
        M[ypxl1+1][xpxl1] = fpart(yend) * xgap;
    }
    intery = yend + gradient;
    
    // handle second endpoint
    int xpxl2,ypxl2;
    xend = roundf(x2);
    yend = y2 + gradient * (xend - x2);
    xgap = fpart(x2 + 0.5);
    xpxl2 = xend;
    ypxl2 = ipart(yend);
    // plot some stuff
    printf("xend = %f, yend = %f, xgap = %f\n",xend,yend,xgap);
    printf("xpxl2 = %d, ypxl2 = %d\n",xpxl2,ypxl2);
    if (flag) {
        M[xpxl2][ypxl2] = rfpart(yend) * xgap;
        M[xpxl2][ypxl2 + 1] = fpart(yend) * xgap;
    } else {
        M[ypxl2][xpxl2] = rfpart(yend) * xgap;
        M[ypxl2 + 1][xpxl2] = fpart(yend) * xgap;
    }
    
    // main loop
    int x, y;
    for (x = xpxl1 + 1; x < xpxl2; x++) {
        y = ipart(intery);
        if (flag) {
            M[x][y] = rfpart(intery);
            M[x][y + 1] = fpart(intery);
        } else {
            M[y][x] = rfpart(intery);
            M[y + 1][x] = fpart(intery);
        }
        intery += gradient;
    }
}


// TODO error checking to make sure that even if end points are beyond image it is drawn without causing trouble
void RMap_drawLineP2PSAFE(RMap *this, float x1, float y1, float x2, float y2) {
    
    float **M = this->grid->val;
    int rows = this->grid->rows;
    int columns = this->grid->columns;
    
    short flag = TRUE;
    float dx,dy, gradient;
    dx = x2 - x1;
    dy = y2 - y1;
    if (fabsf(dx) < fabsf(dy)) {
        swap(&x1,&y1);
        swap(&x2,&y2);
        swap(&dx,&dy);
        flag = FALSE;
    }
    if (x2 < x1) {
        swap(&x1,&x2);
        swap(&y1,&y2);
    }
    gradient = dy/dx;
    
    printf("Do these look right 2 [ %f, %f] [%f, %f]\n",x1,y1,x2,y2);
    // handle the first endpoint
    int xpxl1,ypxl1;
    float xend,yend, xgap, intery;
    xend = roundf(x1);
    yend = y1 + gradient * (xend - x1);
    xgap = rfpart(x1 + 0.5);
    xpxl1 = xend; 
    ypxl1 = ipart(yend);
    // plot some stuff for this 
    printf("xend = %f, yend = %f, xgap = %f, gradient = %f\n",xend,yend,xgap, gradient);
    printf("xpxl1 = %d, ypxl1 = %d\n",xpxl1,ypxl1);
    
    if (flag && (xpxl1 < rows) && (xpxl1 >= 0)) {
        if ((ypxl1 < (columns)) && (ypxl1 >= 0)) {
            M[xpxl1][ypxl1] = rfpart(yend) * xgap;
        }
        if ((ypxl1 < (columns - 1)) && (ypxl1 >= -1)) {
            M[xpxl1][ypxl1+1] = fpart(yend) * xgap;
        }
    } else if ((xpxl1 < columns) && (xpxl1 >= 0)) {
        if ((ypxl1 < rows) && (ypxl1 >= 0)) {
            M[ypxl1][xpxl1] = rfpart(yend) * xgap;
        }
        if ((ypxl1 < (rows - 1)) && (ypxl1 >= -1)) {
            M[ypxl1+1][xpxl1] = fpart(yend) * xgap;
        }
    }
    intery = yend + gradient;
    
    // handle second endpoint
    int xpxl2,ypxl2;
    xend = roundf(x2);
    yend = y2 + gradient * (xend - x2);
    xgap = fpart(x2 + 0.5);
    xpxl2 = xend;
    ypxl2 = ipart(yend);
    // plot some stuff
    printf("xend = %f, yend = %f, xgap = %f\n",xend,yend,xgap);
    printf("xpxl2 = %d, ypxl2 = %d\n",xpxl2,ypxl2);
    
    if (flag && (xpxl2 < rows) && (xpxl2 >= 0)) {
        if ((ypxl2 < (columns)) && (ypxl2 >= 0)) {
            M[xpxl2][ypxl2] = rfpart(yend) * xgap;
        }
        if ((ypxl2 < (columns - 1)) && (ypxl2 >= -1)) {
            M[xpxl2][ypxl2+1] = fpart(yend) * xgap;
        }
    } else if ((xpxl2 < columns) && (xpxl2 >= 0)) {
        if ((ypxl2 < rows) && (ypxl2 >= 0)) {
            M[ypxl2][xpxl2] = rfpart(yend) * xgap;
        }
        if ((ypxl2 < (rows - 1)) && (ypxl2 >= -1)) {
            M[ypxl2+1][xpxl2] = fpart(yend) * xgap;
        }
    }
    
    // main loop
    int x, y;
    for (x = xpxl1 + 1; x < xpxl2; x++) {
        y = ipart(intery);
        if (flag && (x < rows) && (x >= 0))  {
            if ((y < columns) && (y >= 0)) {
                M[x][y] = rfpart(intery);
            }
            if ((y < (columns - 1)) && (y >= -1)) {
                M[x][y + 1] = fpart(intery);
            }
        } else if ((x < columns) && (x >= 0)) {
            if ((y < rows) && (y >= 0)) {
                M[y][x] = rfpart(intery);
            }
            if ((y < (columns - 1)) && (y >= -1)) {
                M[y + 1][x] = fpart(intery);
            }
        }
        intery += gradient;
    }
}

void swap(float *a, float *b) {
    float temp;
    temp = *b;
    *b = *a;
    *a = temp;
}

int ipart(float x) {
    float ans;
    modff(x,&ans);
    return (int) ans;
}

float fpart(float x) {
    float temp;
    return modff(x, &temp);
}

float rfpart(float x) {
    float ans, temp;
    ans = modff(x, &temp);
    return 1 - ans;
}


        
    







